//------------------------------------------------------------------------------
// Fichiers d'entete utilises
//------------------------------------------------------------------------------
#include <stdlib.h>

//------------------------------------------------------------------------------
// Constantes
//------------------------------------------------------------------------------
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include "Vision/Camera.hpp"
#include "Vision/VisionTools.hpp"
#include "Tools/LoggerConsole.hpp"
#include "Calibration/Dataset.hpp"
#include "Calibration/PositionSet.hpp"
#include "Calibration/Processor.hpp"
#include "Calibration/Pattern.hpp"
#include "Viewer/Camera.hpp"

#define MOVING_INCREMENT 0.5
#define DEFAULT_WIDTH 640
#define DEFAULT_HEIGHT 480
#define NEAR_VALUE 0.1f
#define FAR_VALUE 10000.0f


#define PATTERN_ROW 4
#define PATTERN_COLUMN 5
#define PATTERN_SIZE 2.5

#define SIZE_FACTOR 10

// Information sur la zone de visualisation
int viewportX		= 0;
int viewportY		= 0;
int viewportWidth	= DEFAULT_WIDTH;
int viewportHeight	= DEFAULT_HEIGHT;

Viewer::Camera camera;

int currentImageIndex = 0;
Calibration::CalibrationDataset dataset;
Calibration::CalibrationPositionSet positionset;

void Process()
{	
	dataset.AddImageFromFile( "Images/Picture 02.jpg" );
	dataset.AddImageFromFile( "Images/Picture 01.jpg" );
	dataset.AddImageFromFile( "Images/Picture 03.jpg" );
	dataset.AddImageFromFile( "Images/Picture 04.jpg" );
	dataset.AddImageFromFile( "Images/Picture 05.jpg" );
	dataset.AddImageFromFile( "Images/Picture 06.jpg" );
	//dataset.AddImageFromFile( "Images/Picture 16.jpg" );
	dataset.AddImageFromFile( "Images/Picture 17.jpg" );
	
	Tools::ILogger::GetLogger("Console")->Info( "Dataset filled\n" );

	Vision::Camera camera( "Camera" );
	Tools::ILogger::GetLogger("Console")->Info( "Camera created\n" );

	Calibration::Pattern pattern( PATTERN_ROW, PATTERN_COLUMN, PATTERN_SIZE );
	Tools::ILogger::GetLogger("Console")->Info( "Pattern created\n" );
	
	Tools::ILogger::GetLogger("Console")->Info( "Start calibration\n" );
	try
	{
		Calibration::CalibrationProcessor::Instance().CalibrateCamera( pattern, positionset, dataset, camera );

		Calibration::CalibrationPositionSet::Position* position;
		for( int index=0; index < dataset.GetNumberOfImages(); ++index)
		{
	
			position = positionset.GetPosition( index );
			Tools::ILogger::GetLogger("Console")->Info( "------------------------------\n" );
			Tools::mat44_print(position->translation);
			Tools::mat44_print(position->rotation);
			Vision::VisionTools::DrawImage( "Calibration::FindPattern", 500, dataset[ index ] );
		}
	}
	catch( ... )
	{
		Tools::ILogger::GetLogger("Console")->Info( "Error\n" );
	}	
}

void keyboard(unsigned char key, int x, int y);
void reshape(int width, int height);
void display();
void DrawGrid( int gridSize );
void DrawReferential( int referentialSize );
void DrawPattern( int numberOfRows, int numberOfColumns, float offsetX, float offsetY, float patternSize );

int main( int argc, char** argv ) 
{	
	Tools::ILogger::AddLogger( new Tools::LoggerConsole("Console") );
	Tools::ILogger::GetLogger("Console")->Info( "Start program\n" );

	// Initialistaion de la scene
	Process();

    	// Initialisation de GLUT
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE);
	glutInitWindowSize(DEFAULT_WIDTH,DEFAULT_HEIGHT);
	glutCreateWindow("RayTracer");
	glutKeyboardFunc(&keyboard);
	glutDisplayFunc(&display);
	glutReshapeFunc(&reshape);
	glutMainLoop();

	Tools::ILogger::GetLogger("Console")->Info( "End program\n" );
	Tools::ILogger::DeleteLoggers();

	return EXIT_SUCCESS;
}


//------------------------------------------------------------------------------
// Keyboard
//----------------------------------------------------------------------------
void keyboard(unsigned char key, int x, int y)
{

	switch (key)
	{
		case 'z' :
			camera.MoveDeeply( MOVING_INCREMENT );
			break;
		case 's' :
			camera.MoveDeeply( -MOVING_INCREMENT);
			break;
		case 'q' :
			camera.MoveHorizontaly( -MOVING_INCREMENT);
			break;
		case 'd' :
			camera.MoveHorizontaly( MOVING_INCREMENT);
			break;
		case 'n' :
			camera.MoveVertical( -MOVING_INCREMENT);
			break;
		case 'u' :
			camera.MoveVertical( MOVING_INCREMENT);
			break;
		case 'j' :
			camera.RotateCamera( 1.0f, 0.0f );
			break;
		case 'k' :
			camera.RotateCamera( -1.0f,0.0f);
			break;
		case '+' :
			currentImageIndex = currentImageIndex<dataset.GetNumberOfImages()-1?++currentImageIndex:currentImageIndex;
			break;
		case '-' :
			currentImageIndex = currentImageIndex>0?--currentImageIndex:currentImageIndex;
			break;

		case '\x1B':
			exit(EXIT_SUCCESS);
			break;
	}
}

//------------------------------------------------------------------------------
// Reshape
//----------------------------------------------------------------------------
void reshape(int width, int height)
{
	viewportX	= 0;
	viewportY	= 0;
	viewportWidth	= width;
	viewportHeight	= height;
}

//------------------------------------------------------------------------------
// Display rendered image
//----------------------------------------------------------------------------
void display()
{
	// Initialisation d'OpenGL pour avoir une projection orthogonale
	glClearColor(0.0, 0.0, 0.0, 0.0);
	glClearDepth(1.0);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	
	// Initialisation de la projection perspective
	glViewport(0,0,viewportWidth,viewportHeight);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective( camera.FieldOfView, float(viewportWidth)/float(viewportHeight), NEAR_VALUE, FAR_VALUE );
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	
	// Positionnement de la camera de debug
	gluLookAt( 	camera.Position[0], camera.Position[1], camera.Position[2],
			camera.Position[0] + camera.Direction[0], camera.Position[1] + camera.Direction[1], camera.Position[2] + camera.Direction[2],
			camera.Up[0], camera.Up[1], camera.Up[2] );
	
	// 
	DrawGrid( 20 );
	DrawReferential( 2 );
	DrawPattern( PATTERN_ROW, PATTERN_COLUMN, -PATTERN_SIZE, -(PATTERN_ROW-1)*PATTERN_SIZE, PATTERN_SIZE/* / SIZE_FACTOR*/ );

	//Calibration::CalibrationDataset dataset;	
	Calibration::CalibrationPositionSet::Position * position = positionset.GetPosition( currentImageIndex );

	float translationCopy[16];
	float rotationCopy[16];

	Tools::mat44_copy(translationCopy, position->translation);
	translationCopy[3]  /= -SIZE_FACTOR; // Mirroir
	translationCopy[7]  /= -SIZE_FACTOR; // Mirroir
	translationCopy[11] /=  SIZE_FACTOR;
	
	//Tools::mat44_inverse( rotationCopy, position->rotation );

	
	//glRotatef( 180, 0, 1, 0 );
	glMultMatrixf( position->rotation );	
	glMultMatrixf( translationCopy );

	DrawReferential( 2 );

	glutSwapBuffers();
	glutPostRedisplay();
}


void DrawGrid( int gridSize )
{
	// Specification de la couleur a utiliser
	glColor3f(1,1,1);
	
	// Dessing des colonnes
	for( int indexLine = -gridSize; indexLine <= gridSize; ++indexLine )
	{
		glBegin( GL_LINES );
		glVertex3i( indexLine, 0, -gridSize );
		glVertex3i( indexLine, 0,  gridSize );
		glEnd();
	}
	
	// Dessin des lignes
	for( int indexLine = -gridSize; indexLine <= gridSize; ++indexLine )
	{
		glBegin( GL_LINES );
		glVertex3i( -gridSize, 0, indexLine);
		glVertex3i(  gridSize, 0, indexLine);
		glEnd();
	}
}

void DrawPattern( int numberOfRows, int numberOfColumns, float offsetX, float offsetY, float patternSize )
{
	// Dessing des colonnes
	float currentPointX;
	float currentPointY;

	bool drawBlack = true;	
	for( float y= 0; y < numberOfRows*patternSize; y += patternSize )	
	{
		currentPointY = offsetY+y;
		for( float x= 0; x < numberOfColumns*patternSize; x += patternSize )
		{
			currentPointX = offsetX+x;			

			drawBlack = drawBlack?false:true;	
			if( drawBlack )
			{	glColor3f(0.1f,0.1f,0.1f); }
			else
			{	glColor3f(1.0f,1.0f,1.0f); }
			
			glBegin( GL_QUADS );
				glVertex3f( currentPointX, currentPointY, 0);
				glVertex3f( currentPointX+patternSize, currentPointY, 0);
				glVertex3f( currentPointX+patternSize, currentPointY+patternSize, 0);
				glVertex3f( currentPointX, currentPointY+patternSize, 0);
			glEnd();
		}	
		if( numberOfColumns%2 == 0 )
		{	drawBlack = drawBlack?false:true; }
	}
}

void DrawReferential( int referentialSize )
{
	glBegin(GL_LINES);
		// Axe X
		glColor3f(1,0,0);
		glVertex3f( 0,0,0 );
		glVertex3f( referentialSize,0,0 );
	
		// Axe Y
		glColor3f(0,1,0);
		glVertex3f( 0,0,0 );
		glVertex3f( 0,referentialSize,0 );
	
		// Axe Z
		glColor3f(0,0,1);
		glVertex3f( 0,0,0 );
		glVertex3f( 0,0,referentialSize );
	glEnd();
}
